function [F, J, H] = finite_difference(x, fun,Restlength,D,m,g)
% Calculates the Jacobian of function "fun" in "x" using finite differences
% F: the function value in "x"
% J: Jacobian of "fun" in "x"


% Definining the step length
h = sqrt(eps);
t = 10^(-100);

% We evaluate this only once
F = fun(x,Restlength,D,m,g);

n_x = length(x);

J = zeros(length(F), n_x);

% In each loop we obtain one column of the Jacobian
% This is the seed
p = zeros(n_x, 1);
% for k = 1 : n_x
%     p(k) = 1*h;
%     J(:, k) = (fun(x + p,Restlength,D,m,g) - F)/h;
%     p(k) = 0;
% end

% We use the MATLAB imaginary trick to obtain better accuracy
for k = 1 : n_x
    p(k) = 1*t;
    J(:, k) = imag(fun(x + sqrt(-1)*p,Restlength,D,m,g))/t;
    p(k) = 0;
end

h = power(eps, 1/4);
hh = h * h;
%h = sqrt(eps);
% If Hessian is also requested
if nargout == 3
   H = cell(length(F), 1);
   for i = 1 : length(F)
      H{i} = zeros(n_x, n_x);
   end
   p_i = zeros(n_x, 1);
   p_j = zeros(n_x, 1);
   
   for i = 1 : n_x
      p_i(i) = h;
      for j = i : n_x
         p_j(j) = h;
         v = (fun(x + p_i + p_j,Restlength,D,m,g) - fun(x + p_i,Restlength,D,m,g) - fun(x + p_j,Restlength,D,m,g) + F) / hh;
         for k = 1 : length(F)
            H{k}(i, j) = v(k);
         end
         p_j(j) = 0;
      end
      p_i(i) = 0;
   end
   % Calculate lower triangle
   for i = 1 : length(F)
      H{k} = H{k} + (H{k} - diag(diag(H{k})))';
   end
end
